#include "WProgram.h"
#include <math.h>

#include "config.h"
#include "mirf.h"
#include "packet.h"

// ---------------------------------------------------------------------
// Pin Assignments
// ---------------------------------------------------------------------

// analog inputs:
// 0-3 referenced to AVCC
// 4-5 referenced to VCC (noisier?)
// 6-7 only available in 32-pin package
#define TEMPERATURE_PIN  0
#define ACPOWER_PIN      1
#define LIGHTING_PIN     2

// PWM outputs (3,5,6,9,10,11 but 5,6 shared with timing routines and 10,11 used for SPI)
#define AMBER_LED_PIN    3
#define GREEN_LED_PIN    9

// Serial Peripheral Interface pins (SPI)
#define SPI_SSEL        10
#define SPI_MOSI        11
#define SPI_MISO        12
#define SPI_SCLK        13

// digital I/O: use pins 2,4,7,8
// reserved: 0,1(UART); 3,5,6,9(PWM); 10-13(SPI)
#define PIEZO_PIN        2
#define CLICK_LED        4
#define SWITCH_PIN       7
#define STROBE_PIN       8
#define NORDIC_CE        5

// ---------------------------------------------------------------------
// Lighting analysis parameters
// ---------------------------------------------------------------------
#define NLIGHTSAMP 250
#define NLIGHTSAMPBY2 125
#define NLIGHTSAMPBY4 63
#define MINUNCLIPPED 15
#define ADCMIN 0
#define ADCMAX 1005
// The index period corresponding to 120 Hz = 2pi/(NLIGHTSAMP/6)
#define DPHI120 0.15079644737231007

// ---------------------------------------------------------------------
// AC power analysis parameters
// ---------------------------------------------------------------------
#define NPOWERSAMP 250
#define NPOWERSAMPBY2 125
#define NPOWERSAMPBY4 63
// The index period corresponding to 60 Hz = 2pi/(NPOWERSAMP/3)
#define DPHI60 0.075398223686155036
// The overall power conversion factor in Watts(rms)/ADC
// calculated as 120V(rms)*sqrt(2)/NPOWERSAMP*(5000mV)/(1024ADC)/(90mV/A(pk))
#define POWERSCALE 0.036828478186799352
// Power to threshold conversion factor: (1<<27)/(1000W)^2
#define THRESHOLDSCALE 134.21772799999999

// ---------------------------------------------------------------------
// Temperature monitoring parameters
// ---------------------------------------------------------------------
#define NTEMPSUM        2048
#define NTEMPSUMBY2     1024
// (0.1F/mV)(5000mV/1024ADC)/NTEMPSUM
#define ADC2DEGF 0.0002384185791015625
// 2pi/NTEMPSUM
#define DPHIGLOW 0.0030679615757712823

// =====================================================================
// Global variable declarations. All variables must fit within 2K
// of SRAM, including any variables allocated on the stack at runtime.
// =====================================================================

Packet packet;

// ---------------------------------------------------------------------
// Shared globals
// ---------------------------------------------------------------------
byte byteValue;
unsigned int uintValue;
float floatValue;
unsigned long ulongValue;

// ---------------------------------------------------------------------
// Globals used for 60/120 Hz waveform captures (lighting + AC power)
// ---------------------------------------------------------------------
unsigned int buffer[512],*bufptr,delayCycles;
byte counter = 0;

#define WAVEDATA(K) buffer[(((K)+6)<<1)|1]

// ---------------------------------------------------------------------
// Lighting and power analysis globals
// ---------------------------------------------------------------------
byte cycle,nzero;
float sink,cosk,beta0,beta1,beta2,cosSum,sinSum;
float alpha00,alpha01,alpha02,alpha11,alpha12,alpha22;

// ---------------------------------------------------------------------
// Temperature monitoring globals
// ---------------------------------------------------------------------
byte whichLED;
unsigned int temperatureIndex;
unsigned long temperatureSum;

// ---------------------------------------------------------------------
// Globals used by the Geiger tick routine
// ---------------------------------------------------------------------
byte doClick;
unsigned long randomState = 24071966UL;
unsigned long clickThreshold = 0;

// ---------------------------------------------------------------------
// Globals used by the printFloat() function
// ---------------------------------------------------------------------
unsigned long multiplier;

// =====================================================================
// Print a floating point value as a fixed-precision decimal.
// Uses Serial.write() and Serial.print(). Does not emit a newline.
// Uses the shared globals floatValue and ulongValue.
// =====================================================================

#define printFloat(x,m) { floatValue = (x); multiplier = (m); _printFloat(); }

void _printFloat() {
    // convert +/-ABC.XYZ to ABCXYZ with rounding
    if(floatValue < 0) {
        ulongValue = -floatValue*multiplier + 0.5;
        if(ulongValue) Serial.write('-');
    }
    else {
        ulongValue = floatValue*multiplier + 0.5;
    }
    // emit the integral part ABC
    Serial.print(ulongValue/multiplier,DEC);
    // emit the fractional part .XYZ
    Serial.write('.');
    ulongValue %= multiplier;
    // left pad the fractional part XYZ with zeros if necessary
    while(multiplier > 10) {
        multiplier /= 10;
        if(ulongValue >= multiplier) break;
        Serial.write('0');
    }
    Serial.print(ulongValue,DEC);
}

// =====================================================================
// Delays for about 1ms and generates an audible and visible "Geiger"
// click at pseudo-random intervals with an average rate controlled
// by the value of the clickThreshold global.
// =====================================================================
void tick() {
    // generate a pseudo-random unsigned long
    randomState = randomState*2891336453UL + 1640531513UL;
    randomState ^= randomState >> 13;
    randomState ^= randomState << 17;
    randomState ^= randomState >> 5;
    doClick = (randomState < clickThreshold) ? 1 : 0;
    digitalWrite(CLICK_LED,doClick);
    delayMicroseconds(250);
    digitalWrite(PIEZO_PIN,doClick);
    delayMicroseconds(500);
    digitalWrite(PIEZO_PIN,LOW);
    delayMicroseconds(250);
    digitalWrite(CLICK_LED,LOW);
}

// =====================================================================
// Analyzes a lighting waveform to determine its mean and 120Hz
// amplitude, using a linear least-squares technique that is robust
// against clipping and can reconstruct waveforms whose baseline exceeds
// the ADC input range as long as some 120Hz is visible within the range.
// This function makes the following math calls: 62*sin, 62*cos, 3*sqrt.
// =====================================================================
void lightingAnalysis() {
    nzero = 0;
    alpha00 = NLIGHTSAMP;
    alpha11 = alpha22 = NLIGHTSAMPBY2;
    alpha01 = alpha02 = alpha12 = 0;
    beta0 = beta1 = beta2 = 0;
    for(byteValue = 0; byteValue < NLIGHTSAMPBY4; byteValue++) {
        if(byteValue == 0) {
            sink = 0;
            cosk = 1;
        }
        else {
            floatValue = DPHI120*byteValue;
            sink = sin(floatValue);
            cosk = cos(floatValue);
        }
        for(cycle = 0; cycle < 4; cycle++) {
            if((cycle%2) && (byteValue==0)) continue;
            switch(cycle) {
                case 0:
                uintValue = WAVEDATA(byteValue);
                break;
                case 1:
                uintValue = WAVEDATA(NLIGHTSAMPBY2-byteValue);
                break;
                case 2:
                uintValue = WAVEDATA(NLIGHTSAMPBY2+byteValue);
                break;
                case 3:
                uintValue = WAVEDATA(NLIGHTSAMP-byteValue);
                break;
            }
            if(uintValue > ADCMIN && uintValue < ADCMAX) {
                floatValue = uintValue;
                beta0 += floatValue;
                beta1 += floatValue*cosk;
                beta2 += floatValue*sink;
            }
            else {
                if(uintValue <= ADCMIN) nzero++;
                alpha00 -= 1;
                alpha01 -= cosk;
                alpha02 -= sink;
                alpha11 -= cosk*cosk;
                alpha12 -= sink*cosk;
                alpha22 -= sink*sink;
            }
            sink = -sink;
        }
    }
    
    // Check for an almost singular matrix which signals an over/under-flow condition
    // in the sampling. At this point, alpha00 contains the number of unclipped samples
    // out of a possible NLIGHTSAMP maximum.
    if(alpha00 < MINUNCLIPPED) {
        beta1 = 0;
        if(nzero > NLIGHTSAMPBY2) {
            // underflow
            beta0 = 0;
        }
        else {
            // overflow
            beta0 = -1;
        }
    }
    else {
        // Calculate the Cholesky decomposition of the symmetric postitive definite matrix alpha(i,j).
        // Replace each alpha(i,j) with the lower matrix element L(j,i).
        alpha00 = sqrt(alpha00);
        alpha01 /= alpha00;
        alpha02 /= alpha00;
        alpha11 = sqrt(alpha11-alpha01*alpha01);
        alpha12 = (alpha12-alpha01*alpha02)/alpha11;
        alpha22 -= alpha02*alpha02+alpha12*alpha12; // this is actually L(2,2)^2
    
        // Solve the linear equations alpha . x = beta for the vector of coeficients x.
        // Replace each beta(i) with x(i)
        beta0 /= alpha00;
        beta1 = (beta1 - alpha01*beta0)/alpha11;
        beta2 = (beta2 - alpha02*beta0 - alpha12*beta1)/alpha22;
        beta1 = (beta1 - alpha12*beta2)/alpha11;
        beta0 = (beta0 - alpha01*beta1 - alpha02*beta2)/alpha00;
    
        // beta0 is the mean lighting level in ADC counts. Clip it at zero.
        if(beta0 < 0) beta0 = 0;
        // Calculate the 120Hz peak amplitude in ADC counts and store in beta1
        beta1 = sqrt(beta1*beta1 + beta2*beta2);
        // Decide if artificial light is present
        if(beta1 > 2 && beta1 > 0.1*beta0) {
            cycle = 1;
        }
        else {
            cycle = 0;
        }
    }
}

// =====================================================================
// Analyzes an AC current waveform to determine its true RMS power
// relative to a 120V RMS 60Hz voltage. The algorithm assumes that
// there is no clipping and that the load is purely resistive.
// This function makes the following math calls: 62*sin, 62*cos, 1*sqrt.
// =====================================================================
void powerAnalysis() {
    cosSum = sinSum = 0;
    for(byteValue = 0; byteValue < NPOWERSAMPBY4; byteValue++) {
        if(byteValue == 0) {
            sink = 0;
            cosk = 1;
        }
        else {
            floatValue = DPHI60*byteValue;
            sink = sin(floatValue);
            cosk = cos(floatValue);
        }
        for(cycle = 0; cycle < 4; cycle++) {
            if((cycle%2) && (byteValue==0)) {
                cosk = -1;
                continue;
            }
            switch(cycle) {
                case 0:
                uintValue = WAVEDATA(byteValue);
                break;
                case 1:
                uintValue = WAVEDATA(NPOWERSAMPBY2-byteValue);
                cosk = -cosk;
                break;
                case 2:
                uintValue = WAVEDATA(NPOWERSAMPBY2+byteValue);
                sink = -sink;
                break;
                case 3:
                uintValue = WAVEDATA(NPOWERSAMP-byteValue);
                cosk = -cosk;
                break;
            }
            floatValue = uintValue;
            cosSum += floatValue*cosk;
            sinSum += floatValue*sink;
        }
    }
    // store the RMS power (Watts) in cosSum
    cosSum = POWERSCALE*sqrt(cosSum*cosSum+sinSum*sinSum);
    // update the click threshold based on the new power estimate
    clickThreshold = (unsigned long)(THRESHOLDSCALE*cosSum*cosSum);
}

// =====================================================================
// The setup() function is called once on startup.
// =====================================================================

void setup() {
    pinMode(GREEN_LED_PIN,OUTPUT);
    pinMode(STROBE_PIN,OUTPUT);
    pinMode(PIEZO_PIN,OUTPUT);
    pinMode(CLICK_LED,OUTPUT);
    pinMode(SWITCH_PIN,INPUT);
    // startup the serial port
    Serial.begin(9600);
    Serial.println("hello");
    // blink CLICK_LED to signal the program start: short-short-long
    digitalWrite(CLICK_LED,HIGH);
    delay(500);
    digitalWrite(CLICK_LED,LOW);
    delay(500);
    digitalWrite(CLICK_LED,HIGH);
    delay(500);
    digitalWrite(CLICK_LED,LOW);
    delay(500);
    digitalWrite(CLICK_LED,HIGH);
    delay(1500);
    digitalWrite(CLICK_LED,LOW);
    delay(500);
    
    // nordic wireless initialization
    Mirf.csnPin = SPI_SSEL;
    Mirf.cePin = NORDIC_CE;
    Mirf.init();

    byteValue = Mirf.getStatus();
    Serial.print("Nordic SR=");
    Serial.println(byteValue,BIN);
    
    Mirf.setTADDR(HUB_ADDRESS);
    Mirf.payload = sizeof(Packet);
    Mirf.channel = RADIO_CHANNEL;
    Mirf.config();
    Serial.println("Nordic configured");
    
    // initialize wireless packet
    packet.deviceID = 0xbeef;
    packet.sequenceNumber = 0;
    packet.packetType = PACKET_SENSOR_READINGS;
    for(byteValue = 0; byteValue < PACKET_VALUES; byteValue++) {
        packet.data[byteValue] = 0;
    }
}

void nordicTestLoop() {
}

void loop() {

    // LEDs off during light measurements
    digitalWrite(AMBER_LED_PIN,LOW);
    digitalWrite(GREEN_LED_PIN,LOW);

    // =====================================================================
    // Do a burst of 256 x 5kHz ADC samples lasting exactly 51,200 us
    // 250 samples span exactly 6 120Hz powerline cycles
    // =====================================================================
    bufptr = buffer;
    noInterrupts();
    do {
        // toggle pin13 to allow scope timing measurements
        digitalWrite(STROBE_PIN, HIGH);
        *bufptr++ = TCNT0;
        *bufptr++ = analogRead(LIGHTING_PIN);
        digitalWrite(STROBE_PIN, LOW);
        // insert some idle delay (borrowed from delayMicroseconds() in wiring.c)
        delayCycles = 328; // 4 CPU cycles = 0.25us per iteration
        __asm__ __volatile__ (
            "1: sbiw %0,1" "\n\t" // 2 cycles
            "brne 1b" : "=w" (delayCycles) : "0" (delayCycles) // 2 cycles
            );
    } while(++counter); // wraps around at 256
    interrupts();

    // Read the input switch
    if(!digitalRead(SWITCH_PIN)) {
        Serial.print("LOG lwave");
        for(uintValue = 0; uintValue < 250; uintValue++) {
            Serial.write(' ');
            Serial.print(WAVEDATA(uintValue));
        }
        Serial.println();
    }

    // Analyze the captured waveform and dump the results
    lightingAnalysis();
    
    /*
    Serial.print("LOG light ");
    printFloat(beta0,10);
    Serial.write(' ');
    printFloat(beta1,10);
    Serial.write(' ');
    Serial.println(cycle,DEC);
    */
    packet.data[0] = (unsigned int)(10*beta0);
    packet.data[1] = (unsigned int)(10*beta1);

    // =====================================================================
    // Calculate average of NTEMPSUM temperature ADC samples.
    // Result is stored in 32-bit unsigned, so can average up to 2^22
    // 10-bit ADC samples without overflow. The temperature sampling is
    // split into two sets, with the LED ramping on during the first set
    // and off during the second set. AC power sampling is performed
    // between the two sets. The green LED is used if there is no artificial
    // lighting detected, and otherwise the amber LED is used.
    // =====================================================================
    
    temperatureSum = 0;
    if(cycle) {
        whichLED = AMBER_LED_PIN;
    }
    else {
        whichLED = GREEN_LED_PIN;
    }

    //----------------------------------------------------------------------
    // Temperature sampling set 1
    //----------------------------------------------------------------------
    // The first sample sometimes reads low so don't use it
    analogRead(TEMPERATURE_PIN);
    tick();
    for(temperatureIndex = 0; temperatureIndex <  NTEMPSUMBY2; temperatureIndex++) {
        temperatureSum += analogRead(TEMPERATURE_PIN);
        // gradually ramp the LED on during the first set of temperature samples
        analogWrite(whichLED,(int)(127.*(1.-cos(temperatureIndex*DPHIGLOW))+0.5));
        tick();
    }

    //----------------------------------------------------------------------
    // Do a burst of 256 x 5kHz ADC samples lasting exactly 51,200 us
    // 250 samples span exactly 3 60Hz powerline cycles
    //----------------------------------------------------------------------
    bufptr = buffer;
    noInterrupts();
    do {
        // toggle pin13 to allow scope timing measurements
        digitalWrite(STROBE_PIN, HIGH);
        *bufptr++ = TCNT0;
        *bufptr++ = analogRead(ACPOWER_PIN);
        digitalWrite(STROBE_PIN, LOW);
        // insert some idle delay (borrowed from delayMicroseconds() in wiring.c)
        delayCycles = 328; // 4 CPU cycles = 0.25us per iteration
        __asm__ __volatile__ (
            "1: sbiw %0,1" "\n\t" // 2 cycles
            "brne 1b" : "=w" (delayCycles) : "0" (delayCycles) // 2 cycles
            );
    } while(++counter); // wraps around at 256
    interrupts();
    
    // Read the input switch
    if(!digitalRead(SWITCH_PIN)) {
        Serial.print("LOG pwave");
        for(uintValue = 0; uintValue < 250; uintValue++) {
            Serial.write(' ');
            Serial.print(WAVEDATA(uintValue));
        }
        Serial.println();
    }
    
    // Analyze the captured waveform and dump the results
    powerAnalysis();
    
    /*
    Serial.print("LOG power ");
    printFloat(cosSum,100);
    Serial.write(' ');
    Serial.print(clickThreshold,DEC);
    Serial.println();
    */
    packet.data[2] = (unsigned int)(100*cosSum);
    
    //----------------------------------------------------------------------
    // Temperature sampling set 2
    //----------------------------------------------------------------------
    // The first sample sometimes reads low so don't use it
    analogRead(TEMPERATURE_PIN);
    tick();
    for(temperatureIndex = NTEMPSUMBY2; temperatureIndex < NTEMPSUM; temperatureIndex++) {
        temperatureSum += analogRead(TEMPERATURE_PIN);
        // gradually ramp the LED off during the second set of temperature samples
        analogWrite(whichLED,(int)(127.*(1.-cos(temperatureIndex*DPHIGLOW))+0.5));
        tick();
    }

    /*
    Serial.print("LOG temp ");
    printFloat(temperatureSum*ADC2DEGF,1000);
    Serial.println();
    */
    packet.data[3] = (unsigned int)(100*temperatureSum*ADC2DEGF);
    
    // Append the nordic transmit observer register contents to our packet
    Mirf.readRegister(OBSERVE_TX,(byte*)&(packet.data[7]),1);
    
    // Transmit our new sensor readings
    packet.sequenceNumber++;
    Mirf.send((byte*)&packet);
    while(Mirf.isSending()) { }
}

int main(void) {
    init();
    setup();
    for (;;) {
        loop();
    }
    return 0;
}